//
// Created by jimmyteng on 19-6-8.
//

#ifndef VISUALCOMPASS_VISUAL_COMPASS_H
#define VISUALCOMPASS_VISUAL_COMPASS_H

#include <string>
#include <opencv2/opencv.hpp>
#include "utils/base_macro_define.h"

class VisualCompass {
 SINGLETON_MACRO(VisualCompass)
 public:
  /// Init with K and DistCoef
  /// \param K  Camera calibration of intrinsic parameters
  /// \param DistCoef Camera distortion parameters
  /// \return Error Code if < 0 error
  int Initialize(const cv::Mat &K, const cv::Mat &DistCoef, const cv::Mat &R);

  /// Detect Orientation form Mat
  /// \param image Input image
  /// \param degree Detection Result
  /// \param Confidence Result Confidence
  /// \return Error Code if < 0 error
  int Detect(const cv::Mat &image, double &degree, double &Confidence);

  /// Get Initialized Flag
  /// \return bool flag
  bool IsInitFlg() const {
    return init_flg_;
  }
  /// Get Binary Image To Debug
  /// \return
  const cv::Mat &GetBinaryImage() const {
    return binary_image_;
  }

  /// Get Small Window Size
  /// \return Small window size
  uint GetSmallWindowSize() const {
    return small_window_size_;
  }

  /// Set Small Window Size
  /// \param small window size
  void SetSmallWindowSize(uint small_window_size) {
    small_window_size_ = small_window_size;
  }

  /// Get Big Window Size
  /// \return Big Window Size
  uint GetBigWindowSize() const {
    return big_window_size_;
  }

  /// Set Big Window Size
  /// \param big_window_size
  void SetBigWindowSize(uint big_window_size) {
    big_window_size_ = big_window_size;
  }

  /// Get Detection Threthold
  /// \return
  double GetDetectionThrethold() const {
    return detection_threthold_;
  }

  /// Set Detection Threthold
  /// \param detection_threthold
  void SetDetectionThrethold(double detection_threthold) {
    detection_threthold_ = detection_threthold;
  }

  /// Get Confidence Threthold
  /// \return
  double GetConfidenceThrethold() const {
    return confidence_threthold_;
  }

  /// Set Confidence Threthold
  /// \param confidence_threthold
  void SetConfidenceThrethold(double confidence_threthold) {
    confidence_threthold_ = confidence_threthold;
  }

 private:
  int _DetectOrientation(double &degree, double &Confidence);
  int _ImagePreprocess(const cv::Mat &image);

  // photo size and offset
  static constexpr unsigned int width_ = 640;
  static constexpr unsigned int height_ = 480;
  static constexpr unsigned int width_offset_ = 80;
  static constexpr unsigned int height_offset_ = 0;
  static constexpr unsigned int roi_width_ = width_ - 2 * width_offset_;
  static constexpr unsigned int roi_height_ = height_ - 2 * height_offset_;
  static constexpr unsigned int number_angle_ = 90;
  static constexpr unsigned int number_rho_ = ((roi_width_ + roi_height_) >> 1) + roi_width_ + roi_height_ + 2;

  //init flg
  bool init_flg_ = false;
  //parameter
  uint small_window_size_ = 1;
  uint big_window_size_ = 5;
  double detection_threthold_ = 1;
  double confidence_threthold_ = 3.0;
  double r_ = 15.0;
  // camera and lens parameters
  cv::Mat K_ = cv::Mat();               //camera calibration matrix
  cv::Mat DistCoef_ = cv::Mat();        //lens distortion coefficient
  cv::Mat R_ = cv::Mat();               //R Matrix
  cv::Mat mUndistMap1_ = cv::Mat();
  cv::Mat mUndistMap2_ = cv::Mat();

  // image for debug
  cv::Mat diff_image_ = cv::Mat();
  cv::Mat binary_image_ = cv::Mat();

  // sum data
  double sin_[number_angle_ << 1] = {0.0};
  double cos_[number_angle_ << 1] = {0.0};
  int accumulative_table_[number_angle_][number_rho_] = {0};
  double res[number_angle_] = {0.0};
};

#endif //VISUALCOMPASS_VISUAL_COMPASS_H
